#!/usr/bin/env python3

import rospy
from DrEmpower_can.msg import impedance_control_multi
# import DrEmpower as dr

def callback(data):
    import DrEmpower as dr
    # 将所有变量转换为列表形式
    data.angle_list = list(data.angle_list)
    data.speed_list = list(data.speed_list)
    data.tff_list = list(data.tff_list)
    data.kp_list = list(data.kp_list)
    data.kd_list = list(data.kd_list)
    
    dr.impedance_control_multi(id_list=data.id_list, angle_list=data.angle_list, speed_list=data.speed_list, 
                               tff_list=data.tff_list, kp_list=data.kp_list, kd_list=data.kd_list,mode=data.mode)
    rospy.loginfo("impedance_control_multi_node")


def listener():
    rospy.init_node("impedance_control_multi", anonymous=True)
    rospy.Subscriber("impedance_control_multi", impedance_control_multi, callback)
    rospy.spin()


if __name__ == '__main__':
    listener()

